import math
import struct

#快速平方根倒数
def Q_rsqrt(number):
    """
    快速计算 1/√number 的近似值。

    参数:
        number: 正浮点数

    返回:
        1/√number 的近似值
    """
    threehalfs = 1.5

    x2 = number * 0.5
    y  = number
    # 将浮点数的二进制表示转换为整数
    packed_y = struct.pack('f', y)
    i = struct.unpack('I', packed_y)[0]
    # 魔法常数，见于 Quake III 的实现
    i = 0x5f3759df - (i >> 1)
    # 将整数转换回浮点数
    packed_i = struct.pack('I', i)
    y = struct.unpack('f', packed_i)[0]
    # 牛顿迭代，1次迭代通常就已经足够了
    y = y * (threehalfs - (x2 * y * y))
    return y

def turn_imu_data(imu_data):
    """
    将陀螺仪数据转换成真实物理数据
    
    参数：
        imu_data: 陀螺仪get()数据
        
    返回：
        真实数据构成的元组
    """
    ax = imu_data[0]*0.244/1000
    ay = imu_data[1]*0.244/1000
    az = imu_data[2]*0.244/1000
    gx = imu_data[3]*0.07*0.0174
    gy = imu_data[4]*0.07*0.0174
    gz = imu_data[5]*0.07*0.0174
    
    return (ax,ay,az,gx,gy,gz)



accex = 0
accey = 0
accez = 0
q0 = 1
q1 = 0
q2 = 0
q3 = 0
Ki=  0.001
dt = 0.005
Kp  =1.0
def angle_cal(turn_imu):
    """
    四元数解算
    
    参数:
        turn_imu: 转换后陀螺仪数据元组
    
    return:
        pitch, roll, yaw
    """
    global accex
    global accey
    global accez
    global q0
    global q1
    global q2
    global q3
    global Ki
    global Kp
    global dt
    
    ax,ay,az,gx,gy,gz = turn_imu
    

        
       
    norm = Q_rsqrt(ax*ax+ay*ay+az*az)
    ax *= norm
    ay *= norm
    az *= norm
        
    vx = 2*(q1*q3-q0*q2)
    vy = 2*(q0*q1+q2*q3)
    vz = 1-2*q1*q1-2*q2*q2
        
    ex = ay*vz - az*vy
    ey = az*vx - ax*vz
    ez = ax*vy - ay*vx
        
    accex  += ex*Ki*dt
    accey  += ey*Ki*dt
    accez  += ez*Ki*dt
        
    gx += Kp*ex+accex
    gy += Kp*ey+accey
    gz += Kp*ez+accez
        
    gx *= 0.005
    gy *= 0.005
    gz *= 0.005
        
    q0_, q1_, q2_, q3_ = q0, q1, q2, q3
        
    q0 += -q1_*gx - q2_*gy - q3_*gz
    q1 +=  q0_*gx + q2_*gz - q3_*gy
    q2 +=  q0_*gy - q1_*gz + q3_*gx
    q3 +=  q0_*gz + q1_*gy - q2_*gx
        
    norm = Q_rsqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3)
        
    q0 *= norm
    q1 *= norm
    q2 *= norm
    q3 *= norm
        
    g1 = 2 * (q1*q3 - q0*q2)
    g2 = 2 * (q0*q1 + q2*q3)
    g3 = q0*q0 - q1*q1 - q2*q2 + q3*q3
    g4 = 2 * (q1*q2 + q0*q3)
    g5 = q0*q0 + q1*q1 - q2*q2 - q3*q3

    pitch = -math.asin(g1) * 57.3
    roll = math.atan2(g2, g3) * 57.3
    yaw = math.atan2(g4, g5) * 57.3
    
    return (pitch,roll,yaw)
    
  
    
    
    
    












